/**
 * test all the workflow of Tracker
 */

#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "ygz/Frame.h"
#include "ygz/Feature.h"
#include "ygz/Settings.h"
#include "ygz/ORBExtractor.h"
#include "ygz/ORBMatcher.h"
#include "ygz/EurocReader.h"
#include "ygz/Tracker.h"
#include "ygz/TrackerLK.h"
#include "ygz/BackendSlidingWindowG2O.h"
#include "ygz/Viewer.h"

string leftFolder = "/var/dataset/euroc/MH_03_medium/cam0/data";
string rightFolder = "/var/dataset/euroc/MH_03_medium/cam1/data";
string imuFolder = "/var/dataset/euroc/MH_03_medium/imu0/data.csv";
string timeFolder = "/home/xiang/code/ygz-stereo-inertial/examples/EuRoC_TimeStamps/MH03.txt";
string configFile = "/home/xiang/code/ygz-stereo-inertial/examples/EuRoC.yaml";
string vocFile = "/home/xiang/code/ygz-stereo-inertial/Vocabulary/ORBvoc.bin";

int start_index = 0; // 起始图像的索引

using namespace ygz;

int main(int argc, char **argv) {

    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimeStamp;
    VecIMU vimus;

    LoadImages(leftFolder, rightFolder, timeFolder, vstrImageLeft, vstrImageRight, vTimeStamp);
    LoadImus(imuFolder, vimus);

    // Read rectification parameters
    cv::FileStorage fsSettings(configFile, cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "ERROR: Wrong path to settings" << endl;
        return 1;
    }

    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() ||
        D_r.empty() ||
        rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return 1;
    }

    cv::Mat M1l, M2l, M1r, M2r;
    cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l,
                                M2l);
    cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r,
                                M2r);

    const int nImages = vstrImageLeft.size();

    // Create camera object
    setting::initSettings();
    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];
    float bf = fsSettings["Camera.bf"];

    shared_ptr<CameraParam> camera(new CameraParam(fx, fy, cx, cy, bf));

    // create a tracker
    shared_ptr<TrackerLK> tracker(new TrackerLK());
    tracker->SetCamera(camera);

    // create a backend
    shared_ptr<BackendSlidingWindowG2O> backend(new BackendSlidingWindowG2O(tracker));
    tracker->SetBackEnd( backend );

    // Main loop
    cv::Mat imLeft, imRight, imLeftRect, imRightRect;
    size_t imuIndex = 0;

    // set up visualization
    shared_ptr<Viewer> viewer(new Viewer(true));
    tracker->SetViewer(viewer);

    double tstart = vTimeStamp[0];

    for (int ni = start_index; ni < nImages; ni++) {

        // Read left and right images from file
        LOG(INFO)<<"Loop "<<ni<<endl;
        imLeft = cv::imread(vstrImageLeft[ni], CV_LOAD_IMAGE_UNCHANGED);
        imRight = cv::imread(vstrImageRight[ni], CV_LOAD_IMAGE_UNCHANGED);

        cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR);

        VecIMU vimu;

        double tframe = vTimeStamp[ni];

        while (1) {
            const ygz::IMUData &imudata = vimus[imuIndex];
            if (imudata.mfTimeStamp >= tframe)
                break;
            vimu.push_back(imudata);
            imuIndex++;
        }

        LOG(INFO)<<"put into tracker "<<endl;
        SE3d Twb = tracker->InsertStereo(imLeftRect, imRightRect, tframe, vimu);

        /*
        cv::Mat img = viewer->DrawImage();
        cv::imshow("current", img);
        cv::waitKey(0);
         */

    }

    sleep(10);
    setting::destroySettings();
}